/*
 * PSDetectFormation.java
 *
 * Created on 18 de agosto de 2007, 08:35 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors.Schemas.Perception;
import Behaviors.Formations.FormationGenerator;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.Sensors.Vector2DSensor;
import Utils.Vector2d;
import javaclient2.Position2DInterface;
import Robot.Position2d;
import Utils.Pose2d;
/**
 *
 * @author jonathan
 */
public class PSDetectFormation extends Schema implements Vector2dSchema {
    Position2d self;
    Position2d center;
    int robotId;
    FormationGenerator form;

    /**
     * Defines the distance of the formation to the center of the virtual robot.
     */
    private float distToCenter = 0.0f;
    /**
     * Creates a new instance of PSDetectFormation
     */
    public PSDetectFormation(String name, FormationGenerator formation,Position2d self,Position2d center) {
        super(name);
        this.self=self;
        this.center=center;
        robotId=self.getRobotId();
        form=formation;
    }
    
    public Vector2d Value(){
        Pose2d myPosition = self.read(Position2d.READ_MODE_NO_WAIT);
        Pose2d centerPosition = center.read(Position2d.READ_MODE_NO_WAIT);
        
        Vector2d centerPos=new Vector2d(centerPosition.getPx(),centerPosition.getPy(),Vector2d.MODE_XY);
        Vector2d myPos=new Vector2d(myPosition.getPx(),myPosition.getPy(),Vector2d.MODE_XY);
        Vector2d formationPos=form.getPositionForRobot(robotId-1);
        
        
        double a = centerPosition.getPa();
         if(formationPos == null)
         {
             return self.getPosition();
         }
         
        try{
            formationPos=formationPos.rotate(a);
        }
        catch(NullPointerException e){
            System.out.println("formation pos null");
        }
        
        
        Vector2d ret;
        
        Vector2d temp = new Vector2d(distToCenter,centerPosition.getPa(),Vector2d.MODE_MA);
        temp = centerPos.sub(temp); 
        //el formation pos esta mostrando error!!
        ret = temp.add(formationPos);
        //System.out.println(formationPos.toString()+" Robot: "+robotId);

        //System.out.println(robotId);
        //ret.print(Vector2d.MODE_XY);
        return ret;

    }

    /**
     * Sets the distance of the formation to the virtual robot.
     * @param distToCenter Distance (in [m]) to the virtual robot.
     */
    public void setDistToCenter(float distToCenter)
    {
        this.distToCenter = distToCenter;
    }
}
